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Abstract 

We  develop  a  method  that  allows  a  flyer  to  estimate  its  own  motion  (egomotion),  the  wind  velocity, 
ground  slope,  and  flight  height  using  only  inputs  from  onboard  optic  flow  and  air  velocity  sensors.  Our 
artificial  algorithm  demonstrates  how  it  could  be  possible  for  flying  insects  to  determine  their  absolute 
egomotion  using  their  available  sensors,  namely  their  eyes  and  wind  sensitive  hairs  and  antennae.  Al¬ 
though  many  behaviors  can  be  performed  by  only  knowing  the  direction  of  travel,  behavioral  experiments 
indicate  that  odor  tracking  insects  are  able  to  estimate  the  wind  direction  and  control  their  absolute  ego¬ 
motion  (i.e.  groundspeed) .  The  egomotion  estimation  method  that  we  have  developed,  which  we  call 
the  opto-aeronautic  algorithm,  is  tested  in  a  variety  of  wind  and  ground  slope  conditions  using  a  video 
recorded  flight  of  a  moth  tracking  a  pheromone  plume.  Over  all  test  cases  that  we  examined,  the  algo¬ 
rithm  achieved  a  mean  absolute  error  in  height  of  7%  or  less.  Furthermore,  our  algorithm  is  suitable  for 
the  navigation  of  aerial  vehicles  in  environments  where  signals  from  the  Global  Positioning  System  are 
unavailable. 


1  Introduction 

Flying  insects  navigate  proficiently  through  their  environment  using  multiple  sensory  modalities  and  limited 
computational  resources  (Taylor  and  Krapp,  2008).  The  use  of  multiple  sensor  modalities  allows  insects 
to  navigate  in  a  wide  variety  of  environmental  conditions.  Thus,  flying  insects  can  serve  as  inspiration  for 
methods  and  sensory  systems  that  can  be  used  to  navigate  unmanned  aerial  vehicles. 

Flying  insects  use  their  compound  eyes  to  detect  optic  flow,  or  the  apparent  motion  of  visual  patterns 
in  their  visual  field.  The  optic  flow  at  any  point  in  the  visual  field  is  a  combination  of  the  insect’s  own 
translational  and  rotational  motion,  or  egomotion,  and  the  motion,  if  any,  of  the  visual  pattern  at  that  part 
of  the  visual  field.  Optic  flow  is  essentially  an  angular  rate  of  the  relative  motion  of  a  visual  pattern  in  a  given 
viewing  direction.  There  are  several  artificial  methods  of  computing  optic  flow  from  a  vision  sensor.  Barron 
et  al.  (1994)  group  these  techniques  into  differential,  region-based  matching,  energy-based,  and  phase-based 
techniques.  These  techniques  vary  in  computational  complexity,  and  the  accuracy  of  these  techniques  can 
be  dependent  on  the  visual  structure  of  the  scene.  In  our  work,  we  are  not  particularly  concerned  with  how 
optic  flow  is  computed.  Instead,  we  will  focus  on  using  optic  flow  in  higher  levels  of  processing  to  estimate 
egomotion. 

At  the  next  level  of  processing,  the  optic  flow  field,  which  consists  of  a  collection  of  optic  flow  vectors  along 
multiple  viewing  directions,  is  used  to  estimate  egomotion.  If  everything  in  the  environment  is  stationary, 
the  rotational  egomotion  and  the  direction  of  translational  egomotion  can  be  computed  from  the  optic  flow 
field,  but  the  magnitude  of  translational  egomotion  (i.e.  groundspeed)  is  inversely  related  to  the  distance 
of  the  scene  from  the  visual  system  (Koenderink  and  van  Doom,  1987).  For  instance,  Miao  et  al.  (1996) 
implemented  a  differential  optic  flow  technique  to  estimate  these  quantities  from  a  recorded  image  sequence 
from  a  forward  looking  camera  mounted  aboard  a  helicopter.  The  computation  of  egomotion  from  a  spherical 
motion  field  has  been  formulated  by  Koenderink  and  van  Doom  (1987),  and  a  simpler,  noniterative  procedure 
was  developed  by  Dahmen  et  al.  (2001).  Srinivasan  (1994)  developed  the  image  interpolation  technique  to 
estimate  egomotion  directly  from  a  sequence  of  image  intensity  patterns  from  a  monocular  camera  without 
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the  intermediate  step  of  computing  the  optic  flow  in  multiple  viewing  directions.  This  technique  was  extended 
by  Bab-Hadiashar  et  al.  (1995)  and  Nagle  and  Srinivasan  (1996)  to  allow  for  the  estimation  of  the  component 
of  translational  egonrotion  perpendicular  to  the  visual  system,  and  further  extended  to  the  full  6  degree- 
of-freedom  case  by  Nagle  and  Srinivasan  (1997).  However,  the  fundamental  problem  with  using  vision  to 
estimate  egonrotion  is  that  the  groundspeed  cannot  be  determined  unless  the  distance  to  features  in  the 
visual  field  is  known  or  can  be  measured  or  estimated. 

In  the  UAV  literature,  several  approaches  have  been  taken  to  resolve  the  scale  ambiguity  problem  that 
is  inherent  with  a  visual  system.  Recently,  Qelik  et  al.  (2009)  have  proposed  an  approach  for  absolute 
distance  and  groundspeed  estimation  using  only  a  monocular  camera;  however,  it  is  assumed  that  the  flight 
height  is  known.  Franz  et  al.  (2004)  have  developed  a  method  of  distance  and  groundspeed  estimation  using 
an  omnidirectional  camera,  but  knowledge  of  the  average  scene  distance  is  required.  Flight  height  can  be 
measured  with  a  downward  looking  distance  measurement  sensor,  such  as  a  sonar  sensor  (Conroy  et  al., 
2009)  or  a  laser  rangefinder  (Garratt  and  Chahl,  2008).  Another  approach  is  to  use  stereo  vision  (Toupet 
et  al.,  2007).  It  has  also  been  common  to  use  an  inertial  measurement  unit  to  aid  a  vision  based  navigation 
system  to  determine  the  distance  to  the  scene.  However,  none  of  these  engineering  solutions  is  used  by 
flying  insects.  Since  insects  have  fixed  focus  optics,  closely  spaced  eyes,  and  typically  only  a  small  region  of 
visual  overlap  between  their  two  eyes,  their  stereoscopic  vision  is  only  useful  within  a  range  of  a  couple  of 
centimeters  (Srinivasan  et  al.,  1999).  Insects  possess  sensors  for  measuring  angular  rates  (Pringle,  1948;  Sane 
et  al.,  2007);  however,  a  sensor  for  measuring  linear  accelerations  has  not  yet  been  found  in  insects.  If  a  flying 
insect  could  somehow  estimate  range  by  combining  the  information  from  its  visual  system  with  information 
from  another  of  its  available  sensors,  then  it  could  also  estimate  the  magnitude  of  its  translational  egomotion. 

Flying  insects  use  a  combination  of  their  antennae  and  various  types  of  nreclranosensory  hairs  to  detect  air 
currents  (Gewecke,  1974).  As  an  insect  flies,  the  air  current  that  it  senses  is  a  combination  of  its  egomotion 
and  the  wind.  In  the  biological  literature,  this  relationship  is  referred  to  as  the  triangle  of  velocities  (Marsh 
et  al.,  1978).  In  aeronautics,  it  is  simply  called  the  wind  triangle  (Comeaux,  1983).  The  velocity  of  the  insect 
relative  to  a  stationary  point  on  the  ground  is  the  ground  vector,  vg,  the  velocity  of  the  air  mass  relative  to 
a  stationary  global  reference  frame  is  the  wind  vector,  w,  and  the  velocity  of  the  insect  relative  to  the  air 
mass  is  the  air  vector,  va.  The  wind  vector,  ground  vector,  and  air  vector  are  related  at  any  point  in  time, 
t,  through  the  vector  equation  (1). 


w (t)=vg(t) -va(t)  (1) 

There  is  evidence  that  some  insects  can  control  their  groundspeed  in  an  absolute  sense,  particularly  in 
insects  that  use  pheromones  to  attract  and  find  mates.  In  several  insect  species,  a  male  insect  locates  a 
mate  by  flying  upwind  when  it  smells  the  sex-attractant  pheromone  released  by  a  female  of  the  same  species 
(Kennedy  and  Marsh,  1974).  The  speed  at  which  they  progress  upwind  while  tracking  an  odor  (i.e.  the 
upwind  component  of  the  ground  vector)  is  nearly  the  same  in  any  wind  speed,  thus  these  insects  do  not 
fly  at  a  preferred  airspeed  (Willis  and  Arbas,  1991;  Zanen  and  Carde,  1996;  Foster  and  Howard,  1999). 
If  odor  tracking  insects  were  flying  so  as  to  maintain  a  preferred  optic  flow  across  their  retinas,  then  it 
would  be  expected  that  the  ground  vector  would  increase  proportionally  with  odor  plume  altitude.  However, 
a  proportional  relationship  between  the  ground  vector  and  odor  plume  altitude  has  never  been  observed 
for  any  odor  tracking  insect  [moths  Heliothis  virescens  and  Grapholita  molesta  (Kuenen  and  Baker,  1982), 
Epiphyas  postvittana  (Foster  and  Howard,  1999),  beetles  Prostephanus  truncatus  (Fadanriro  et  al.,  1998),  or 
wasps  Microplitis  croceipes  (Zanen  and  Carde,  1996)].  Furthermore,  during  odor  tracking,  an  insect  must 
be  able  to  resolve  the  wind  vector  from  the  air  vector,  thus,  from  (1),  it  must  also  be  able  to  determine  its 
ground  vector. 

We  have  developed  a  method  that  simultaneously  estimates  egomotion,  wind  velocity,  flight  height,  and 
ground  slope  using  measurements  from  onboard  air  velocity  and  optic  flow  sensors.  We  call  this  algorithm 
the  opto-aeronautic  algorithm.  The  validity  of  our  estimator  is  tested  in  a  variety  of  wind  and  ground  slope 
conditions. 
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2  Approach 

We  divide  our  approach  into  three  sections.  First,  we  present  a  simplified  formulation  of  the  opto-aeronautic 
algorithm.  Next,  we  present  an  extended  formulation  of  the  algorithm  that  relaxes  several  of  our  assumptions 
that  were  made  in  our  simplified  formulation.  Finally,  we  present  the  procedure  that  we  use  to  test  the 
extended  formulation  of  the  algorithm  using  simulated  and  experimentally  collected  data. 

2.1  Simplified  formulation 

To  introduce  our  approach,  we  begin  with  the  simplified  problem  of  determining  egomotion  when  flying 
directly  forward  in  a  constant  wind  over  a  flat  ground.  Onboard  sensors  measure  the  air  vector  and  the 
optic  flow  field.  To  estimate  egomotion,  we  make  the  following  assumptions,  which  will  also  be  true  in  the 
extended  formulation. 

1.  The  air  vector  and  optic  flow  are  measured  simultaneously  and  sampled  at  a  constant  rate.  This 
assumption  is  made  only  to  simplify  the  development  of  the  algorithm.  The  time  between  samples,  or 
time  step  length,  is  At. 

2.  There  is  no  lag  or  bias  in  the  optic  flow  and  air  velocity  sensors.  Any  real  sensor  will  have  lag,  however, 
any  lag  that  is  present  in  the  sensors  will  simply  introduce  lag  in  the  estimation  of  egomotion,  but  will 
not  affect  the  quality  of  the  estimate. 

3.  All  visible  objects  are  stationary,  thus  any  optic  flow  is  induced  solely  through  the  motion  of  the  visual 
system.  If  the  field  of  view  of  the  visual  system  is  sufficiently  wide,  then  objects  that  are  moving  in 
the  environment  will  contribute  very  little  to  the  estimation  of  egomotion. 

4.  Only  the  downward  looking  portion  of  the  optic  flow  field  is  used  to  estimate  egomotion  since  this 
portion  of  the  visual  field  is  known  to  contribute  to  speed  control  in  flying  insects  (Kennedy,  1940; 
Kennedy  and  Marsh,  1974). 

We  define  the  apparent  egovelocity  vector,  7,  as  an  estimate  of  the  apparent  velocity  scaled  with  respect 
to  the  current  height.  The  apparent  egovelocity  is  obtained  directly  from  the  optic  flow  field,  for  example, 
by  using  the  image  interpolation  technique  of  Srinivasan  (1994).  In  our  simplified  problem,  we  assume  that 
the  ground  vector,  or  in  this  case,  groundspeed,  is  directly  equal  to  the  product  of  the  apparent  egovelocity 
and  the  height.  Thus,  the  wind  is  related  to  the  apparent  egovelocity,  height,  and  airspeed  through  equation 
(2). 


w  =  7/1  -  va  (2) 

Notice  that  this  equation  has  two  unknown  quantities,  w  and  h,  which  cannot  be  uniquely  determined 
at  a  single  instant  in  time.  We  could  uniquely  solve  for  estimates  of  the  wind  speed  and  flight  height,  w  and 
h.  by  using  (2)  to  create  a  system  of  two  equations  at  two  different  points  in  time.  However,  the  estimates 
for  wind  speed  and  flight  height  would  be  highly  dependent  on  noise  in  the  measurements  of  7  and  va ■  To 
reduce  the  effects  of  sensor  measurement  noise  on  the  estimation  process,  we  solve  for  w  and  h  in  a  least 
squares  sense.  First,  we  collect  measurements  of  7  and  va  over  a  short  length  of  time  and  store  them  in 
batch  vectors,  T  and  V0,  as  given  in  (3)  and  (4).  These  batch  vectors  can  be  thought  of  as  a  short-term 
memory  of  the  sensor  measurements.  We  refer  to  this  length  of  time  as  the  batch  window,  which  contains  n 
measurements . 


r=[7(*r)  •••  7 (t„)f  (3) 

Va  =  [Va(tl)  ■  .  .  Va(tn)]T  (4) 

From  (2),  we  formulate  an  estimate  of  the  wind  over  all  points  in  the  batch  window,  W,  in  the  following 
manner. 

w  =  rh  -  va  (5) 
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Since  we  know  in  this  case  that  the  wind  is  constant,  we  can  use  (6)  as  a  model  for  the  wind,  where  W 
is  a  batch  vector  that  contains  the  value  of  the  wind  model  at  each  point  in  the  batch  window,  bw  is  an 
unknown  wind  model  coefficient,  and  1  is  a  column  vector  of  all  ones. 


W  =  1  bw  (6) 

Ideally,  the  wind  estimate,  W,  and  the  wind  model,  W,  should  be  equal  at  all  points  in  the  batch  window. 
However,  since  there  may  be  noise  in  the  sensors,  we  solve  for  values  of  bw  and  h  that  minimize,  in  a  least 
squares  sense,  the  difference  between  the  wind  estimate  and  the  wind  model.  If  we  define  the  wind  error, 
Ew,  as  in  (7),  where  ||  •  ||  represents  the  2-norm  or  Euclidean  norm,  we  can  formulate  a  linear  least  squares 
problem,  (8),  to  solve  for  bw  and  h,  which  for  our  simplified  problem  takes  the  form  of  (9). 


Ew  =  W  —  W 

(7) 

min  Ew 

(8) 

II  -  ii2 

min  16w  —  Th  —  Va 

(9) 

h,bw 


To  solve  the  minimization  problem  of  (9),  first  assume  that  the  height  is  known.  Using  the  Moore-Penrose 
pseudoinverse,  which  is  defined  in  (10)  for  any  matrix  T  with  more  rows  than  columns,  the  wind  coefficient 
is  obtained  from  (11). 

T+  =  (TtT)_1Tt  (10) 


bw  =  l+(Th  -  V„)  =  —  1t(I7i  -  V„) 

n 


(ii) 


By  substituting  (11)  back  into  (9),  and  defining  a  matrix  ©  as  in  (12),  where  I  is  an  n  x  n  identity 
matrix,  we  can  now  solve  for  h  according  to  (13). 


1  T 

0  =  -11T-I 

n 


(12) 


rT©T©va 

rT0T0r 


(13) 


A  solution  for  the  height  estimate  can  only  be  obtained  if  the  groundspeed  is  non-constant.  Otherwise, 
r  will  be  in  the  null  space  of  0,  thus  the  product  of  0  and  T  will  be  zero  and  the  denominator  of  (13)  will 
be  zero.  In  effect,  changes  in  apparent  egovelocity  must  be  correlated  with  changes  in  airspeed  to  deduce 
the  effect  of  the  constant  wind.  The  groundspeed  is  determined  by  multiplying  h  by  7,  and  the  wind  is 
determined  by  substituting  (13)  back  into  (2). 


2.2  Extended  formulation 

We  now  extend  our  formulation  to  consider  the  case  of  motion  in  three  translational  degrees-of-freedom  and 
one  rotational  degree-of-freedom  about  the  y-axis  (yaw)  (Figure  1).  Methods  for  overcoming  the  requirements 
of  zero  pitch  rate  and  zero  roll  rate  are  discussed  later,  but  not  explicitly  included  in  the  formulation.  We 
remove  the  requirement  of  a  constant  wind  by  introducing  a  parameterized  wind  model.  We  will  also  examine 
the  effects  of  noise  in  the  sensor  measurements  on  the  quality  of  the  egomotion  estimates.  In  addition,  we 
will  implement  a  more  complex  optic  flow  model  that  accounts  for  the  effects  of  discrete  time  sampling. 
Furthermore,  we  will  extend  the  basic  approach  to  handle  flight  over  a  sloped  ground. 

Our  opto-aeronautic  algorithm  is  summarized  in  the  block  diagram  in  Figure  2.  The  algorithm  is  divided 
into  three  sections  -  air  velocity  preprocessing,  optic  flow  preprocessing,  and  sensor  fused  estimation.  The  air 
velocity  preprocessing  and  optic  flow  preprocessing  are  independent  and  may  be  performed  simultaneously. 
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Figure  1:  A  flyer  traverses  over  an  inclined  ground.  The  trajectory  flown  is  not  necessarily  straight,  as 
indicated  by  the  curved  path  across  the  ground.  All  variables  are  described  in  the  text. 


Figure  2:  Block  diagram  of  the  information  processing  model  of  the  opto-aeronautic  algorithm 
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Measurements  of  the  air  vector  and  optic  flow  are  made  in  a  body-fixed  reference  frame.  We  use  a  right- 
handed  Cartesian  coordinate  system  to  represent  the  sensor  measurements.  The  coordinate  system  is  fixed 
to  the  visual  system  such  that  the  x-axis  is  aligned  with  a  horizontal  projection  of  the  longitudinal  axis  of 
the  flyer,  the  positive  y-axis  is  directly  upward  (opposes  the  gravity  vector),  and  the  positive  z-axis  points 
out  the  right  side  of  the  body  (Figure  1).  The  first,  second,  and  third  components  of  the  air  vector  and  the 
optic  flow  vector  are  the  x-component,  y-component,  and  z-component,  respectively.  We  place  a  caret  (') 
above  estimated  quantities  to  distinguish  them  from  ’’true”  quantities. 

In  contrast  to  the  simplified  problem,  we  no  longer  necessarily  assume  that  the  wind  speed  and  direction 
are  constant.  Instead,  we  assume  that  the  wind  is  temporally  smooth  over  a  short  time  interval  (the  batch 
window)  and  the  terrain  is  spatially  smooth.  To  implement  the  smoothness  constraints,  we  use  a  sliding 
batch,  or  moving  horizon,  estimation  process  in  discrete  time  and  we  use  batch  vectors  to  organize  the  sensor 
measurements  and  state  estimates.  Let  V  represent  a  batch  vector  that  can  be  replaced  with  Va  (batch  air 
vector),  Vg  (batch  ground  vector),  or  W  (batch  wind  vector).  The  vector  V  with  subscript  contains  the 
ny  most  recent  sensor  measurements  in  the  direction  of  e*,  as  in  (14),  where  the  directions  ei,  e%,  and  e 3 
are  equivalent  to  the  coordinate  system  axes  x,  y,  and  z,  respectively.  The  batch  vectors  for  all  Cartesian 
axes  are  then  stacked  to  form  an  augmented  batch  vector  as  in  (15). 


V 


vek{t  -  {nv  -  1)A t) 


vek{t- At) 
Vek(t) 


<u 

> 

< 

R 

Ve2 

= 

V, 

> 

> 

(14) 


(15) 


We  use  a  left-pointing  arrow  (•<— )  to  indicate  an  update  of  the  batch  vectors  when  proceeding  from  one 
time  step  to  the  next.  For  example,  the  following  statement  indicates  that  the  jth  element  of  the  vectors 
Vx,  Vy,  and  Vz  is  determined  by  multiplying  b  by  the  (j+l)th  element  of  these  vectors  in  the  previous  time 
step. 


Vj  <-  bvj+ 1  (16) 

Since  the  flyer  may  rotate  about  its  vertical  axis,  the  body-fixed  coordinate  system  changes  relative  to 
an  inertial  frame.  A  coordinate  system  fixed  to  the  visual  system  rotates  through  an  angle  Ad  when  moving 
from  one  time  step  to  the  next.  The  differential  angular  displacement  A 8  is  determined  directly  from  the 
optic  flow  field.  The  rotation  matrix,  C,  that  transforms  vectors  from  the  reference  frame  of  the  previous 
time  step  to  the  reference  frame  of  the  current  time  step  is  given  by  (17). 


C  = 


cos(A0)  0  —  sin(A0) 

0  1  0 

sin(A0)  0  cos(A(9) 


(17) 


The  short-term  memory  of  air  vector  measurements  contained  in  the  batch  air  vector  V0  must  be  updated 
so  that  all  vectors  are  represented  in  the  current  body-fixed  coordinate  system.  The  batch  air  vector  is 
updated  across  time  steps  by  using  (18),  where  v(t)  is  the  latest  measurement  of  the  air  vector  taken  at  the 
current  time. 


v 


a,j 


Cv„j+i  1  <  j  <  nVa  -  1 
v(f)  j  =  nVa 


(18) 


In  our  simplified  formulation,  we  assumed  that  the  apparent  egovelocity,  7,  can  be  computed  directly 
from  the  optic  flow  field.  However,  the  optic  flow  field  actually  provides  an  estimate  of  the  apparent  egodis- 
placement  vector,  A,  which  may  be  computed  along  all  three  Cartesian  axes,  for  example,  using  the  technique 
of  Nagle  and  Srinivasan  (1996).  The  apparent  egodisplacement  is  the  sum  of  two  distinct  components  over 
a  short  discrete  time  period.  The  first  component  of  A  comes  from  the  motion  of  the  flyer  relative  to  the 
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inertial  reference  frame.  The  second  component  of  A  comes  from  the  change  in  ground  elevation  as  the  flyer 
traverses  over  the  ground  (Figure  1).  If  the  ground  is  inclined  relative  to  the  viewing  axis  of  the  visual 
system,  the  distance  to  the  ground  will  change  as  the  flyer  traverses  in  the  x  and  z  directions,  even  if  there 
is  no  vertical  component  of  motion.  This  change  will  be  perceived  by  the  visual  system  as  a  change  in  scene 
depth,  or  looming.  If  the  slope  of  the  ground  along  the  direction  of  flight  is  positive  and  the  vertical  position 
of  the  flyer  is  constant,  the  flyer  will  approach  the  ground  and  the  apparent  egodisplacement  in  the  vertical 
direction  will  be  negative. 

Define  a  point  O  as  the  point  on  the  ground  directly  beneath  the  flyer  at  the  current  time  (Figure  1). 
Let  p(t)  denote  the  position  of  the  flyer  at  time  t  relative  to  point  O  and  represented  with  respect  to  the 
body-fixed  coordinate  system  at  the  current  time.  Let  h(t)  denote  the  true  height  of  the  flyer  above  the 
ground,  and  let  hg(t)  denote  the  height  of  the  ground,  relative  to  a  horizontal  reference  plane  passing  through 
point  O.  Thus,  hg  is  defined  to  be  zero  at  the  current  time  step.  The  apparent  egodisplacement  can  be 
defined  using  (19),  where  hg  =  [  0  hg  0  ]T. 


A  (t) 


1 

h(t)At 


p(t)—p(t—At)  _  hg  (t)  — hg  (t—At) 

At  At 

(Ap (t)  -  Ah g(t)) 


1 

h(t)At 


1 

h(t)At 


A px 

A Py  -  A  hg 

Apz 

A px 
Ah 
A pz 


(19) 


We  store  values  of  A  over  a  short  time  period  in  a  batch  vector,  A.  If  we  store  the  apparent  egodisplace¬ 
ment  over  the  batch  window  as  a  function  only  of  the  current  height,  it  will  later  only  be  necessary  to  solve 
for  the  current  height  rather  than  solve  for  the  height  at  each  time  in  the  batch  window.  By  rearranging  the 
expression  for  \y  in  (19),  we  formulate  an  estimate  of  the  height  at  the  previous  time  step  given  an  estimate 
of  the  current  height  and  a  measurement  of  the  apparent  egodisplacement,  as  in  (20). 


h(t  —  At)  =  (1  —  A y(t)  ■  A t)h(t)  (20) 

We  define  a  scale  factor,  Sh,  in  (21),  that  represents  the  relative  change  in  height  from  one  time  step  to 
the  next. 


sh  =  1  -  A y(t)  ■  At  (21) 

Thus,  we  can  update  A  across  time  steps  using  (22). 

A j  i —  .s/,  CAp.  i  1  "A  j  "A  n,\  —  1  (2*2) 

We  now  need  to  estimate  the  apparent  egovelocity,  7,  from  the  egodisplacement  vector.  We  first  define 
the  relative  apparent  position  as  the  summation  of  egodisplacement  from  the  start  of  the  batch  window  up 
to  some  point  in  time.  The  simplest  method  of  estimating  the  apparent  egovelocity  is  to  use  a  backward 
difference  scheme  applied  to  the  relative  apparent  position.  This  is  equivalent  to  setting  the  egovelocity  equal 
to  the  egodisplacement.  The  egovelocity  can  also  be  estimated  from  the  egodisplacement  using  a  central 
difference  scheme,  whereby  the  egovelocity  is  estimated  at  the  middle  of  three  relative  apparent  position 
points  from  the  slope  of  a  line  fit  to  those  three  points.  Higher  order  methods  of  estimating  egovelocity 
from  relative  apparent  position  can  also  be  devised.  In  a  method  described  by  (Lanczos,  1988)  and  used  by 
(Rayner  and  Aldridge,  1985),  velocity  is  calculated  from  the  slope  of  a  parabola  that  is  fit  to  a  neighborhood 
of  five  sampled  position  points  (or  a  neighborhood  of  four  points  near  the  ends  of  the  batch  window).  We 
will  refer  to  this  method  as  the  parabolic  difference  scheme.  One  may  also  choose  not  to  compute  finite 
differences  near  the  endpoints,  resulting  in  a  truncated  set  of  egovelocity  estimates.  The  backward,  central, 
central  truncated,  parabolic,  and  parabolic  truncated  egovelocity  computation  methods  are  summarized  in 
Table  1. 
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Table  1:  Methods  of  computing  apparent  egovelocity,  7,  from  apparent  egodisplacement,  A 
method  apparent  egovelocity  computation  batch  vector  lengths 


nva  =  n 

backward  7„r  =  A,ia  nr  =  n 

rij\  =  1 


central 


Trar  — 1  2^n*  ^”A-l) 

1'nr  =  ^riA 


nVa  =  n 
?rr  =  n 
n  a  =  2 


central  truncated  7„r  =  |(A„a  +  A„a_i) 


parabolic 


7nr-2  —  jyj(2A„A  +  3A„a_i  +  3A„a_2  +  2A„a_3) 

7«r  — 1  —  2(}(11^™a  3AnA_i  T  AAa  —  2 ) 

7«r  —  2o(21A„a  T  8A„a_i  9A„a_2) 


parabolic  truncated  7„r  =  Aj(2A„a  +  3A„a_i  +  3A 


.  o 


9  \ 


nya  =  n 
?rr  =  n  —  1 
n  a  =  2 

fly  =  n  —  1 
?rr  =  n  —  1 
nA  =  4 

fly  =  n  —  1 
nr  =  n  —  3 
nA  =  4 


The  augmented  batch  vector  of  apparent  egovelocity,  T,  is  updated  from  one  time  step  to  the  next  using 
(23).  Notice  that  it  is  not  necessary  to  compute  the  egovelocity  from  the  egodisplacement  over  each  of  the 
nr  samples.  Instead,  only  the  most  recent  values  of  7  need  to  be  computed  from  A  using  one  of  the  apparent 
egovelocity  computation  methods  listed  in  Table  1. 


7 j  shC 7j+i  1  <  j  <  n  -  nA 


(23) 


2.2.1  Ground  shape  model 

In  our  simplified  formulation,  we  assumed  that  the  ground  was  level.  We  now  extend  our  formulation  to 
allow  for  flight  over  an  inclined  ground.  We  consider  two  cases  -  when  the  ground  slope  is  known  and  when 
it  is  not  known.  We  define  a  vector  £  that  contains  the  unknown  quantities  that  must  be  estimated.  In  the 
first  case,  £  represents  the  unknown  height,  in  the  second  case,  £  is  a  vector  containing  estimates  of  both 
the  height  and  the  ground  slope  (Table  2). 

An  estimate  of  the  ground  vector  can  be  computed  from  the  apparent  egovelocity,  an  estimate  of  the 
ground  height,  Hs,  and  the  estimate  of  the  height  of  the  flyer  using  (24)  as  shown  in  Figure  1. 

Vfl  =  rfc-^Hfl  (24) 

If  the  ground  height  is  known,  Hg  is  replaced  with  H9,  otherwise,  a  model  of  the  ground  height  must  be 
used.  The  ground  can  be  modeled  as  a  plane  and  expressed  as  a  linear  function  of  x  and  z.  Although  higher 
order  ground  shape  models  are  certainly  possible,  they  are  not  explored  here.  Let  bgx  denote  the  product  of 
the  height  of  the  flyer,  h,  and  the  slope  of  the  ground  in  the  x-axis  (Figure  1).  Similarly,  define  bgz  in  the 
z-axis  of  the  flyer,  and  let  Bg  =  [  bgx  bgz  ]T.  The  terms  bgx  and  bgz  are  defined  in  this  way  so  that  they 
may  later  be  estimated  from  a  linear  system  of  equations.  The  rate  of  change  in  height  of  the  ground  as  a 
function  of  time  at  the  point  directly  beneath  the  flyer  can  be  written  as  in  (25)  and  (26). 

d  /,  _  dhg  Qx  dhg  Qz 

dtn9  —  dx  dt  '  dz  dt 

h  h  *'25'* 

=  c>a^v  A- 

h  U9X  w  h  Vgz 
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io  _  jl 

Ot  9V  —  dt 


hg(t  —  (n  r  —  1)A  f) 


hg(t  —  At) 


hg(t) 


V  ^z^gz 


(26) 


J^xj  _  _^_TT  _  n 

dtn9x  ~  dtLL9z  ~ 


By  substituting  (26)  into  (24)  and  arranging  in  matrix  form,  we  obtain  the  expressions  for  the  estimate 
of  the  batch  ground  vector,  V9,  given  in  Table  2. 


2.2.2  Wind  model 

The  batch  wind  vector  over  nr  time  steps  is  estimated  using  (27),  where  the  truncated  batch  air  vector,  V'a, 
only  includes  the  first  nr  terms  of  each  Cartesian  component  of  Va. 

W  =  Vg-V'a  (27) 

In  our  simplified  formulation,  we  assumed  that  the  wind  was  constant.  We  now  assume  that  the  wind 
may  be  modeled  by  a  low  order  polynomial  over  the  batch  window.  Let  w  be  a  polynomial  approximation 
to  the  true  wind  vector  w.  The  expression  for  w  is  given  by  (28),  where  m  is  the  degree  of  the  polynomial. 
The  coefficients  of  the  polynomial  curve  are  organized  into  an  augmented  batch  vector  B.ffl  of  length  3(m+l) 
as  given  by  (29). 


w(f  -  jAt)  =  b^o  +  bwi (t  -  jAt)  H - b  b wm(t  -  jAt)m 


(28) 


^wek  — 

bwOek 

bwlek 

k  =  1,  2, 3  B  w  = 

B we2 

bwmek 

-^ice3 

(29) 


From  (28)  and  (29),  we  formulate  a  matrix  equation  that  defines  the  polynomial  approximation  to  the 
wind  vector  over  nr  time  steps.  First,  we  define  the  Vandermonde  matrix  T  as  in  (30),  which  replaces  the 
vector  1  in  (9)  in  the  simplified  formulation  (note  that  1  is  a  Vandermonde  matrix  with  m  =  0). 


T  = 


1  0 

1  At 

1  2A  t 


0 

(At)2 

(2Af)2 


L  1  (nr  —  l)Af  ((nr  —  l)Af)2 


0 

(At)m 

(2At)m 

((nr  -  l)Af)' 


(30) 


The  wind  model  for  each  Cartesian  axis  is  now  formulated  using  (31),  and  the  terms  Wei,  We2,  and 


We3  are  stacked  into  an  augmented  batch  vector  W  of  length  3nr  using  (32). 


Wefc  =  TBwt  A;  =  1,2, 3 


(31) 


'  wei  ‘ 

T 

0 

0 

w  = 

We2 

= 

0 

T 

0 

.  We3  . 

0 

0 

T 

(32) 
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Table  2:  Formulation  of  the  vector  of  unknown  parameters,  £,  the  batch  ground  vector,  Vs,  and  the  coefficient 
matrix,  L 


£  Vg 


L 


ground  slope  known 


h 


ground  slope  unknown 


'  rx  ' 

0 

0  1 

©r* 

Tv 

IN 

h  — 

rx 

0 

1 

u*  ° 

b9 

®  (— bgxYx  +  —  bgzTz ) 

©rz 

rx 

0 

0 

h 

'  ©rx 

0 

0 

r  v 

-rx 

-r* 

bgx 

©r  * 

©IN 

IN 

0 

0 

-  Kz  - 

©IN 

0 

0 

2.2.3  Height  and  ground  slope  estimation 

The  height  and  ground  slope  are  estimated  by  finding  values  for  £  and  that  solve  the  minimization 
problem  of  (8).  If  we  assume  for  the  moment  that  £  is  known,  then  V9  can  be  found  from  Table  2,  W  can 
be  computed  from  (27),  and  (8)  can  be  treated  as  a  simple  linear  least  squares  problem.  The  least  squares 
solution  for  the  wind  coefficient  vector  Bw  is  given  by  (33). 


B 


W 


T  0 
0  T 
0  0 


(33) 


Substituting  the  expression  in  (33)  back  into  (32),  (7),  and  (8)  yields  another  linear  least  squares  problem 
for  where  I  is  an  nr  x  nr  identity  matrix. 


min 

€ 


(TT+  —  I)  0  0 

0  (TT+  - 1)  0 

0  0  (TT+  - 1) 


(34) 


The  minimization  problem  of  (34)  can  be  solved  efficiently  by  first  taking  the  reduced  QR  factorization  of 
T,  where  Qt  is  an  orthonormal  matrix  with  the  same  dimensions  as  T,  and  Rt  is  a  square  upper  triangular 
matrix  with  dimensions  (to  +  1)  x  (to  +  1).  The  expression  TT+  —  I  can  now  be  simplified  as  follows  by 
taking  advantage  of  the  fact  that  an  orthonormal  matrix  satisfies  the  property  that  Qt  j  Qt  =  I- 


TT+  -  I  =  QtQtT  —  I  =  ©  (35) 

Since  T  does  not  depend  on  time  and  At  is  constant,  the  QR  factorization  of  T  only  needs  to  be  performed 
once.  Also,  using  the  matrix  Qt  as  in  (35)  requires  storage  only  for  the  matrix  Qt  rather  than  separate 
matrices  T  and  T+,  and  avoids  the  explicit  calculation  of  the  pseudoinverse,  which  can  be  numerically 
unstable. 

We  may  now  solve  for  £.  We  define  a  matrix  L  representing  the  coefficient  of  £  in  the  minimization  prob¬ 
lem.  By  substituting  the  expressions  for  and  L  from  Table  2  into  (27),  then  into  (33),  the  minimization 
problem  takes  the  form  of  (36),  which  may  be  solved  using  any  linear  least-squares  solution  method. 


£  =  arg  min 

I 


14- 


©  o 
0  © 
0  0 


Notice  that  the  wind  model  coefficients  in  the  vector  B, 
determine  £. 


0 

0 

© 


(36) 


do  not  need  to  be  explicitly  calculated  to 


2.2.4  Ground  vector  and  wind  vector  estimation 

Once  the  height  and  ground  slopes  are  estimated,  the  ground  vector  at  the  current  time  may  be  estimated 
using  (37),  where  bgx  and  bgz  may  be  replaced  with  bgx  and  bgz  if  the  ground  slopes  are  known.  However,  A 
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60 


Figure  3:  Flight  track  of  a  pheromone  tracking  moth.  The  moth  generally  moves  from  right  to  left.  Projec¬ 
tions  of  the  flight  track  onto  horizontal  and  vertical  planes  are  shown  in  gray. 


is  the  apparent  egodisplacement  and  may  be  quite  noisy  since  it  comes  directly  from  the  measured  optic  flow. 
Alternatively,  the  ground  vector  may  be  estimated  from  the  most  recent  value  of  the  apparent  egovelocity,  7, 
as  in  (38).  However,  the  most  recent  egovelocity  estimate  may  not  correspond  to  the  current  time,  depending 
on  the  finite  differentiation  scheme  that  is  used.  We  will  examine  the  usefulness  of  both  of  these  estimators 
in  the  results  section.  Once  an  estimate  of  the  ground  vector  is  obtained,  the  wind  vector  is  estimated  from 
(!)• 


flXyn/^  bgx^xn  a  ^gz^zn\ 

h^zriA 


Vg(t) 


^7xnr 

h'yynr  bgx^Yxnr  bgz'Yznr 

_  h~/zni 


(37) 


(38) 


2.3  Test  procedure 

Position  data  of  a  moth  tracking  an  odor  plume  in  a  wind  tunnel  were  used  to  test  the  opto-aeronautic 
algorithm.  The  data  were  taken  from  an  experiment  in  which  the  moth  was  recorded  with  two  cameras  and 
the  position  of  both  the  head  and  the  tail  were  digitized  at  30  Hz.  This  allowed  the  body  axis  orientation  to 
be  calculated.  The  particular  trial  chosen  had  563  data  points  (N=563)  or  nearly  19  seconds  worth  of  flight 
data  (Figure  3). 

Although  the  moths  in  the  experiment  flew  in  a  constant  wind,  the  performance  of  the  opto-aeronautic 
algorithm  was  tested  with  both  a  constant  wind  and  in  natural  wind  conditions.  Natural  wind  velocity  data 
were  recorded  parallel  to  the  ground  in  an  open  athletic  field  (Van  Horn  Field  at  Case  Western  Reserve 
University)  using  an  orthogonal  set  of  acoustic  anemometers  (Bailey,  2004).  It  is  assumed  that  no  wind 
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Table  3:  Methods  of  computing  the  ground  vector,  v9,  from  position  data,  p 


method 

Vg 

backward 

Vg(tj)  «  l 

f  Pj+1  Pj 
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!  Pi-  Pj  — i 
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II  Al 
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(  At 

(  P.,  +  1  P: 
At 

if  i  =  1 

central 

Vg(tj)  W  < 

P  i + 1 —  Pi  - 
2A  t 

1  if  2  <  i  <  N  — 
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Pi  —  Pi  —  i 

l  At 

II 

•»S 

f  -9Pj+3  +  17pj+2  +  13p3  +  i-21pj 

if  i  =  1 

20At 

Pi+2+7pj'+i+3pj  —  llpj_i 

CM 

II 

20At 

parabolic 

Vg(tj)  «  < 

|  2pj+2+Pj+i— Pi-i— 2pj_2 

if  3  <  j  <  N  —  2 

i 

lOAt 

1  HPi+i  3  Pj  7  p  j  _  i  Pj— 2 

t-H 

II 

•»s 

20A  t 

21pj  —  13p 

j  - 1  —  l^Pi  _2  +9pi  _3 

II 

20At 

smoothing  spline 

GCVSPL  implemented  in  Matlab  (Woltring,  1986;  Reina,  1998) 

existed  in  the  y-direction  (perpendicular  to  the  ground).  The  wind  was  sampled  at  60  Hz  with  a  resolution 
of  approximately  10  cm/s.  The  wind  data  were  then  low  pass  filtered  to  remove  digitization  effects  and 
resampled  at  30  Hz. 

Ground  truth  values  for  the  ground  vector  and  air  vector  were  computed  from  the  wind  velocity  and  moth 
position  data  using  several  different  methods.  The  ground  vector  was  calculated  using  the  backward,  central, 
and  parabolic  ground  vector  estimation  methods  in  Table  3,  and  by  differentiating  a  natural  cubic  smoothing 
spline  fit  to  the  position  data  using  a  Matlab  implementation  of  the  GCVSPL  software  package  (Woltring, 
1986;  Reina,  1998).  Using  these  different  methods  allowed  us  to  detect  any  anomalies  in  the  performance  of 
the  opto-aeronautic  algorithm  that  were  an  artifact  of  the  ground  vector  estimation  method.  The  ground 
vector  was  then  transformed  into  the  body-fixed  coordinate  system  and  used  as  the  true  ground  vector, 
vg.  The  air  vector  was  calculated  by  subtracting  the  wind  vector  from  the  ground  vector.  The  apparent 
egodisplacement,  A,  was  then  calculated  using  (19),  and  the  rotational  angular  egodisplacement,  A 6,  was 
calculated  as  the  angular  difference  in  the  longitudinal  axis  of  the  flyer  in  successive  samples  multiplied  by 
the  sampling  frequency  of  30  Hz. 

The  performance  of  the  opto-aeronautic  algorithm  was  examined  for  both  the  deterministic  (no  noise) 
sensor  case  and  the  stochastic  sensor  case.  In  the  stochastic  case,  the  airdata  and  apparent  egodisplacement 
were  injected  with  uniformly  distributed  noise  prior  to  input  into  our  opto-aeronautic  algorithm.  Since  Franz 
et  al.  (2004)  achieved  a  mean  relative  error  of  7.5%  for  computing  apparent  translational  egomotion  from 
optic  flow,  and  the  moths  in  our  experiment  were  flying  at  a  speed  of  approximately  30  cm/s  at  a  height  of 
roughly  40  cm  above  the  floor,  we  used  an  error  in  the  range  of  ±0.1  radians  per  second  for  the  translational 
component  of  apparent  egomotion.  Franz  et  al.  (2004)  also  achieved  a  mean  error  of  0.01  radians  per  second 
for  rotational  motion,  which  is  negligible.  We  used  airdata  noise  in  the  range  of  ±10  cm/s,  which  roughly 
corresponds  to  the  performance  of  the  acoustic  anemometers  that  we  used  to  collect  the  wind  velocity  data. 

3  Results 

The  mean  absolute  error  in  height,  £/,  was  used  to  assess  the  accuracy  of  the  opto-aeronautic  algorithm 
over  the  length  of  the  flight  track.  Since  the  ground  vector  and  wind  vector  are  calculated  from  the  height 
estimate,  a  low  value  of  Eh  indicates  that  good  estimates  of  the  ground  vector  and  wind  vector  were  obtained. 
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Since  the  algorithm  has  an  initialization  period  during  which  the  batch  measurement  vectors  of  length  n  are 
filled,  the  first  n  estimates  were  excluded  from  the  calculation  of  Eh-  For  a  flight  track  of  N  total  points, 
Eh  is  calculated  using  (39). 


Eh  =  w1 —  J2  I  HU)  ~  h(ti)  | 

N  —  n  ' 

i=n-\- 1 


(39) 


3.1  Constant  wind  condition 

In  this  section,  we  examine  the  accuracy  of  height  estimation  when  the  wind  is  constant  in  the  negative 
x-direction  with  a  speed  of  100  cm/s.  We  plot  the  mean  absolute  error  in  height  as  a  function  of  n  (the 
number  of  points  used  for  the  batch  window)  for  each  of  the  egovelocity  computation  methods  given  in 
Table  1  and  ground  vector  computation  methods  given  in  Table  3  (where  the  ground  vector  computation 
is  used  only  to  generate  the  inputs  of  the  air  vector  and  egodisplacement  vector  as  described  previously). 
Since  the  wind  is  constant,  we  set  in ,  the  degree  of  the  polynomial  approximating  the  wind,  to  zero. 

First,  we  examine  the  case  when  there  is  no  noise  in  the  sensors  (Figure  4).  With  no  sensor  noise,  the 
backward  egovelocity  computation  method  produces  a  value  for  Eh  on  the  order  of  10“ 14  cm  for  all  values 
of  n  when  the  ground  vector  is  computed  using  a  backward  differencing  scheme  (Figure  4a).  The  central 
truncated  egovelocity  computation  method  performs  similarly  when  the  ground  vector  is  computed  using 
a  central  differencing  scheme  (Figure  4b),  and  the  parabolic  truncated  egovelocity  computation  method 
performs  similarly  when  the  ground  vector  is  calculated  using  a  parabolic  differencing  scheme  (Figure  4c). 
This  demonstrates  that  the  algorithm  performs  near  the  limits  of  numerical  precision  when  the  egovelocity 
computation  method  is  a  perfect  model  for  the  ground  vector.  The  non-truncated  egovelocity  computation 
methods  do  not  perform  as  well  because  they  do  not  perfectly  model  the  ground  vector  at  the  endpoints  of 
the  batch  window.  When  the  ground  vector  was  computed  with  a  smoothing  spline,  all  of  the  egovelocity 
computation  methods  produce  errors  in  the  height  estimate  due  to  errors  in  modeling  the  ground  vector 
(Figure  4d).  Even  with  no  sensor  noise,  it  is  evident  that  the  backward  egovelocity  computation  method 
is  a  bad  choice  because  it  performs  poorly  unless  the  backward  ground  vector  computation  model  is  a 
good  model  for  the  true  ground  vector,  which  is  unlikely.  Overall,  the  parabolic  and  parabolic  truncated 
egovelocity  computation  methods  are  quite  robust  to  unmodeled  differences  between  the  true  ground  vectors 
and  their  finite  difference  approximations. 

We  now  separately  examine  the  effects  of  optic  flow  and  air  velocity  sensor  noise  on  the  performance  of 
the  algorithm.  The  resulting  values  for  Eh  with  noise  only  in  the  optic  flow  sensors  are  shown  in  Figure  5, 
and  Figure  6  shows  the  results  with  noise  only  in  the  air  velocity  sensors.  Compared  to  Figure  4,  we  see 
that  adding  a  small  amount  of  noise  to  either  sensor  produces  error  in  the  height  estimate,  even  when  the 
egovelocity  computation  method  matches  the  ground  vector  computation  method.  This  is  not  surprising.  If 
one  examines  the  results  closely  for  the  parabolic  and  parabolic  truncated  egovelocity  computation  methods 
when  the  ground  vector  is  calculated  with  either  the  spline  method  or  the  parabolic  method,  it  appears 
that  there  is  a  limit  to  how  well  the  algorithm  can  perform  as  n  increases.  Overall,  the  parabolic  truncated 
egovelocity  computation  method  is  the  best  choice  since  it  produces  a  value  of  Eh  <  2  cm  for  n  >25  in  all 
tests,  and  it  produces  the  lowest  values  for  Eh  when  the  most  accurate  ground  vector  computation  methods 
are  used  (the  parabolic  and  smoothing  spline  methods). 

3.2  Natural  wind  condition 

We  now  examine  the  performance  of  the  opto-aeronautic  algorithm  with  both  noiseless  and  noisy  sensors  in 
the  varying  natural  wind  measured  at  Van  Horn  Field.  Based  on  our  results  from  tests  in  a  constant  wind, 
we  use  the  parabolic  truncated  egovelocity  computation  method  and  the  spline  ground  vector  computation 
method.  To  determine  the  best  values  to  choose  for  m  (the  degree  of  the  polynomial  approximating  the 
wind)  and  n  (the  number  of  points  used  for  the  batch  window),  the  value  of  m  is  adjusted  from  0  to  4  and 
the  value  of  n  is  adjusted  from  its  lowest  possible  value  (depending  on  the  egovelocity  computation  method 
used)  to  50. 

First,  we  examine  the  case  of  flying  over  level  ground.  In  a  varying  wind,  we  see  the  advantage  of  using 
a  higher  degree  polynomial  model  of  the  wind  (Figure  7).  If  the  wind  is  variable,  the  0th  degree  polynomial 
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Figure  4:  Mean  absolute  error  in  the  height  estimate,  Eh,  as  computed  using  (39)  from  the  output  of  the 
opto- aeronautic  egomotion  algorithm  with  zero  noise  in  the  air  velocity  and  optic  flow  input.  The  results 
are  shown  for  various  values  of  the  batch  window  length,  n,  when  the  ground  vector  is  calculated  from  the 
recorded  flight  track  using  each  of  the  methods  given  in  Table  3:  a  backward  differencing  scheme,  b  central 
differencing  scheme,  c  parabolic  differencing  scheme,  d  smoothing  spline.  The  five  different  methods  listed 
in  Table  1  for  estimating  egovelocity  from  measurements  of  egodisplacement  were  tested,  as  indicated  by  the 
curve  labels. 
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Figure  5:  Error  in  the  height  estimate,  Eh,  as  computed  using  the  opto-aeronautic  egomotion  algorithm  with 
uniformly  distributed  noise  of  ±0.1  rad/s  in  the  translational  components  of  optically  detected  apparent 
egomotion.  Other  details  are  the  same  as  in  Figure  4. 
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Figure  6:  Error  in  the  height  estimate,  Eh,  as  computed  using  the  opto-aeronautic  egomotion  algorithm  with 
uniformly  distributed  noise  of  ±10  cm/s  in  the  air  velocity  measurements.  Other  details  are  the  same  as  in 
Figure  4. 
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Figure  7:  Error  in  height  estimate,  Eh,  for  flight  over  flat  ground  in  a  naturalistic  wind  environment.  The 
results  are  shown  for  various  values  of  the  wind  model  degree,  m,  and  the  batch  window  length,  n.  The 
plots  on  the  left  show  the  results  for  noiseless  sensors,  while  those  on  the  right  show  the  results  for  noisy 
sensors.  The  plots  on  the  top  show  the  performance  of  the  algorithm  when  the  ground  slope  is  known  (thus 
the  parameters  in  the  minimization  problem  of  (36)  are  formulated  according  to  the  first  row  of  Table  2), 
while  those  on  the  bottom  show  the  results  when  the  slope  is  unknown  and  must  be  solved  for  (using  the 
parameters  in  the  second  row  of  Table  2). 

performs  the  best  for  low  values  of  n,  but  at  higher  values  of  n,  the  higher  degree  polynomials  perform 
better.  This  is  reasonable  since  it  is  expected  that  a  polynomial  of  higher  degree  would  be  needed  to  model 
the  wind  over  a  longer  time  period.  In  some  cases,  particularly  when  m  =  1,  increasing  n  will  actually  lead 
to  a  slight  increase  in  Eh .  Adding  noise  to  the  sensors  does  indeed  increase  Eh,  but  only  by  a  small  amount 
for  high  values  of  n.  Thus,  the  estimator  is  fairly  robust  to  sensor  noise,  and  most  of  the  error  in  estimating 
the  height  comes  from  the  inability  of  the  polynomial  wind  model  to  perfectly  model  the  true  wind  profile. 
An  example  of  the  comparison  of  the  height  estimate  to  the  true  height  at  each  point  in  the  flight  track  is 
shown  in  Figure  8.  Notice  that  in  this  case,  the  height  estimate  never  diverges  from  the  true  height  by  more 
than  11.0  cm. 

We  now  examine  the  case  of  flight  over  an  inclined  ground  with  a  slope  in  the  x-direction  of  -0.2  and  a 
slope  in  the  z-direction  of  0.2  (Figure  9).  The  mean  absolute  error  in  height  only  improves  slightly  when 
we  explicitly  solve  for  the  ground  slope  rather  than  assume  that  the  ground  is  level,  and  only  when  n  >  30. 
However,  the  mean  error  in  height  (bias),  as  computed  with  (39)  but  without  taking  absolute  values,  is 
generally  reduced  when  we  explicitly  solve  for  ground  slope.  For  example,  when  m  =  1  and  n  =  50,  the  bias 
in  the  height  estimate  goes  from  1.8  cm  when  the  ground  is  assumed  level  to  only  0.1  mm  when  we  explicitly 
solve  for  the  slope.  The  mean  absolute  error  in  height  is  even  better  if  the  slope  of  the  ground  is  known, 
but  only  by  about  0.5  cm  or  less.  Also,  the  advantage  of  using  a  higher  degree  polynomial  approximation 
to  the  wind  diminishes  when  noise  is  added  to  the  sensor  data.  An  example  of  the  comparison  of  the  height 
and  ground  slope  estimates  to  their  true  values  at  each  point  in  the  flight  track  is  shown  in  Figure  10.  The 
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Figure  8:  Comparison  of  the  estimated  height  to  the  actual  height  for  flight  over  level  ground  when  m  =  1, 
n  =  50,  the  ground  slope  is  unknown,  and  both  sensors  are  corrupted  with  noise.  The  batch  measurement 
vectors  of  length  n  are  filled  during  the  initialization  period  shown  in  gray.  The  mean  absolute  error  in 
height,  Eh,  is  2.0  cm. 


estimated  ground  slope  has  been  transformed  into  the  inertial  reference  frame  so  that  it  is  easy  to  compare 
to  the  true  ground  slope.  Over  the  course  of  the  flight  track,  the  mean  slope  estimates  are  close  to  the 
true  values,  but  the  ground  slope  error  has  a  relatively  large  variance  (slope  in  x:  -0.23±0.12,  slope  in  z: 
0.21±0.05).  Thus,  the  minimization  criterion  of  (36)  is  not  very  sensitive  to  the  estimate  of  ground  slope, 
which  makes  it  difficult  to  obtain  quality  slope  estimates.  A  comparison  of  the  two  different  methods  given  in 
(37)  and  (38)  for  estimating  the  ground  vector  shows  that  the  estimator  of  (38)  provides  smoother  estimates 
of  the  ground  vector  and  the  wind  vector,  but  at  the  expense  of  a  time  lag  of  two  time  steps,  or  0.067  seconds 
(Figure  11). 


4  Discussion 

In  this  work,  we  have  demonstrated  that  it  is  possible,  while  airborne,  to  estimate  absolute  egomotion  (the 
ground  vector),  altitude,  ground  slope,  and  the  wind  vector  using  only  inputs  from  an  onboard  downward¬ 
looking  visual  system  and  onboard  air  velocity  sensors.  The  method  we  have  described  works  in  both  constant 
and  varying  wind  and  even  over  sloped  terrain.  Our  method  also  does  not  require  any  prior  knowledge  of  the 
environment  or  the  flyer  motion  states.  This  work  is  significant  in  two  ways.  First,  it  presents  a  hypothetical 
method  for  flying  insects  to  estimate  their  ground  vector  and  the  wind  direction.  Second,  it  is  a  method  of 
velocity  estimation  that  is  suitable  for  aerial  vehicles  operating  in  environments  where  the  Global  Positioning 
System  (GPS)  signal  is  unavailable. 

Although  odor  tracking  insects  are  clearly  able  to  resolve  the  wind  direction  in  flight,  it  is  not  known 
exactly  how  they  do  this.  It  has  been  suggested  that  odor  tracking  insects  maintain  a  constant  sum  of 
the  transverse  and  longitudinal  components  of  the  optic  flow  vectors  in  the  ventral  portion  of  their  visual 
field  (Ludlow,  1984;  David,  1986).  The  wind  direction  corresponds  to  the  orientation  at  which  the  insect 
detects  minimal  transverse  optic  flow,  or  wind-induced  drift.  However,  a  constant  sum  of  transverse  and 
longitudinal  optic  flow  components  was  not  observed  in  odor  tracking  Manduca  sexta  (Willis  and  Arbas, 
1998).  Furthermore,  the  mechanism  proposed  by  Ludlow  (1984)  ignores  the  relationship  among  the  ground 
vector,  optic  flow,  and  height,  and  also  ignores  the  role  of  air  velocity  sense  organs,  which  are  clearly 
important  for  insect  flight  (Budick  et  al,  2007;  Taylor  and  Krapp,  2008).  Input  signals  from  the  compound 
eyes  and  from  hairs  that  are  sensitive  to  air  currents  are  combined  at  a  very  early  stage  of  processing  in  the 
insect  brain  (Taylor  and  Krapp,  2008).  Perhaps  this  is  where  the  estimation  of  height  and  the  decomposition 
of  the  air  vector  into  ground  vector  and  wind  vector  components  are  performed. 

GPS  is  an  extremely  valuable  tool  for  airborne  navigation.  However,  the  GPS  signal  has  limited  use  in 
cluttered  and  indoor  environments  since  the  signal  may  be  blocked  by  structures.  The  navigation  of  micro 
aerial  vehicles  (MAVs)  in  an  environment  without  GPS  is  particularly  challenging  due  to  the  small  size  of  the 
vehicle,  thus  limiting  the  ability  to  carry  computational  power  or  bulky  sensors.  There  is  limited  potential 
for  the  use  of  stereoscopically  computed  range  since  the  performance  of  a  stereo  pair  of  imagers  depends  on 
the  baseline  separation  distance  between  them  (the  farther  apart,  the  better).  Furthermore,  since  MAVs  are 
more  affected  by  wind  disturbances  than  larger  aircraft,  the  ability  to  detect  the  wind  could  be  used  to  help 
mitigate  its  effects  on  control.  Optic  flow  sensors  have  already  been  used  on  MAVs  (Barrows  et  al.,  2003; 
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Figure  9:  Error  in  height  estimate,  Eh,  as  a  function  of  in  and  n  in  a  variable  wind  over  a  ground  with 
a  slope  of  -0.2  along  the  x-direction  and  0.2  along  the  z-direction.  The  plots  on  the  top  show  the  results 
when  the  ground  is  assumed  to  have  0  slope  (by  setting  bgx  =  bgz  =  0  in  the  parameters  of  the  first  row  of 
Table  2),  the  plots  in  the  middle  row  show  the  results  when  the  ground  slope  is  known,  and  the  plots  on  the 
bottom  show  the  results  when  the  ground  slope  is  unknown  and  must  be  solved  for. 
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Figure  10:  Comparison  of  the  height  and  ground  slope  estimates  to  their  true  values  for  flight  over  an  inclined 
ground  when  m  =  1,  n  =  50,  the  ground  slope  is  unknown,  and  both  sensors  are  corrupted  with  noise.  The 
batch  measurement  vectors  of  length  n  are  filled  during  the  initialization  period  shown  in  gray.  The  mean 
absolute  error  in  height,  Eh,  is  2.9  cm,  the  estimated  ground  slope  in  x  has  a  mean  of  -0.23  and  standard 
deviation  of  0.12  ,  and  the  ground  slope  in  z  has  a  mean  of  0.21  and  standard  deviation  of  0.05. 
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Figure  11:  Comparison  between  the  actual  and  estimated  values  of  the  ground  vector  and  wind  vector 
components  as  computed  from  the  apparent  egodisplacement  measurement  (left,  using  (37))  and  as  computed 
from  the  apparent  egovelocity  estimate  (right,  using  (38)) 
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Ruffier  and  Franceschini,  2004;  Zufferey  and  Floreano,  2005;  Griffiths  et  al.,  2006;  Conroy  et  al.,  2009),  and 
biomimetic  artificial  hair  cells  are  a  promising  technology  for  air  velocity  sensing  on  a  small  platform  (Liu, 
2007).  Thus,  our  opto-aeronautic  algorithm  is  particularly  well  suited  to  the  estimation  of  egomotion  and 
wind  velocity  in  MAVs. 

In  the  development  of  our  opto-aeronautic  algorithm,  we  restricted  our  computation  of  egomotion  to  use 
only  the  downward  looking  portion  of  the  visual  field.  Although  algorithms  exist  for  computing  egomotion 
from  an  omnidirectional  field  of  view,  the  computed  egodisplacement  vector  is  either  scaled  to  have  unit 
length  (Koenderink  and  van  Doom,  1987)  or  requires  knowledge  of  the  average  scene  distance  (Franz  et  al., 
2004).  An  egodisplacement  vector  of  arbitrary  scale  at  each  time  step  is  not  sufficient  for  our  algorithm, 
particularly  in  the  computation  of  the  height  scale  factor,  defined  in  (21),  that  relates  the  height  at  previous 
time  steps  to  the  current  height.  Instead,  the  apparent  egodisplacement  must  be  computed  such  that  it  is 
scaled  with  respect  to  the  distance  to  visual  cues,  which  is  possible  when  the  computation  of  egomotion  is 
restricted  to  a  portion  of  the  visual  field  with  minimal  variation  in  distance. 

Since  we  intend  for  our  opto-aeronautic  algorithm  to  be  implemented  on  an  MAV,  the  algorithm  must  be 
computationally  efficient  so  that  it  can  execute  on  a  processing  unit  with  low  mass  and  low  power  demand. 
To  test  if  our  algorithm  is  suitable  for  such  an  application,  we  implemented  our  algorithm  on  a  16-bit 
microcontroller  (dsPIC30F  series)  available  from  Microchip  Technologies  Inc.  With  m  =  1  (which  assumes 
the  wind  is  linear  over  the  batch  window)  and  without  the  estimation  of  ground  slope,  our  algorithm  could 
operate  at  a  rate  of  60  Hz. 

Although  we  have  ignored  the  contribution  of  roll  and  pitch  on  optic  flow,  this  can  be  corrected  for 
through  the  sensing  of  angular  rates.  Many  flying  insects  possess  structures  for  measuring  their  angular 
velocity.  Flies  use  halteres,  which  are  small  club  shaped  structures  that  beat  at  the  same  frequency  as  the 
wings  (Pringle,  1948).  Recent  work  indicates  that  moths  may  use  their  antennae  to  sense  angular  rates 
(Sane  et  al.,  2007).  These  structures  would  allow  a  flyer  to  resolve  the  ambiguity  of  determining  rotation 
and  translation  from  the  optic  flow  field. 

Although  we  have  demonstrated  that  our  opto-aeronautic  algorithm  provides  good  estimates  of  the 
height,  ground  vector,  and  wind  vector,  we  have  not  yet  developed  a  method  of  estimating  the  accuracy  of 
the  outputs  of  the  algorithm  without  knowledge  of  the  true  state.  This  will  be  necessary  since  higher  level 
functionality  may  depend  on  how  well  the  egomotion  is  known.  Also,  we  plan  to  test  the  performance  of  the 
algorithm  with  real  optic  flow  and  air  velocity  sensing  hardware  on  a  real  flying  platform. 
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